﻿using HalconDotNet;
using IniReadWrite;
using System;
using System.Collections.Generic;
using System.IO;
using System.Linq;
using System.Text;
using System.Threading.Tasks;
using System.Windows.Forms;
using System.Xml.Serialization;
using VisionRoc.Tool;

namespace VisionRoc.Core
{
    //上相机纠偏算法类 注：不同心
    public class RotateUpCameraAlgo
    {

        string RotateUpCameraPath;      // 上相机旋转标定加载数据路径

        string CalibrationXML;         // 上相机旋转标定文件加载路径

        public CalibrationData m_CalibrationData; // 标定数据类

        public RotateUpCameraAlgo(string _Path)
        {
            try
            {
                m_CalibrationData = new CalibrationData();
                //判断文件路径是否存在
                if (!Directory.Exists(_Path))
                {
                    Directory.CreateDirectory(_Path);
                }
                RotateUpCameraPath = _Path + "\\RotateUpCameraPara.ini";
                // 平移旋转标定结果.xml
                CalibrationXML = _Path + "\\平移旋转标定结果.xml";
                if (File.Exists(RotateUpCameraPath))
                {
                    ReadParameter();
                }
                if (File.Exists(CalibrationXML))
                {
                    ReafCalibrationXML(CalibrationXML);
                    //HomMat2D = RotateCalculateAlgo.GenHomMat2D(m_CalibrationData.SX, m_CalibrationData.SY, m_CalibrationData.Theta, m_CalibrationData.Phi, m_CalibrationData.TX, m_CalibrationData.TY);
                }
                InIState = 1;
            }
            catch
            {
                InIState = -1;
            }

        }

        /// <summary>
        /// 类初始化状态  1为初始化成功  -1为初始化失败
        /// </summary>
        public int InIState;

        /// <summary>
        /// 平移旋转矩阵
        /// </summary>
        public HTuple HomMat2D = new HTuple();

        /// <summary>
        /// 补偿上方向标志状态
        /// </summary>
        public string UpFlagState;

        /// <summary>
        /// 补偿下方向标志状态
        /// </summary>
        public string DownFlagState;

        /// <summary>
        /// 补偿左方向标志状态
        /// </summary>
        public string LiftFlagStae;

        /// <summary>
        /// 标志右方向标志状态
        /// </summary>
        public string RightFlagState;

        /// <summary>
        /// X 方向补偿
        /// </summary>
        public double X_Offset;

        /// <summary>
        /// Y 方向补偿
        /// </summary>
        public double Y_Offset;

        /// <summary>
        /// 角度补偿
        /// </summary>
        public double Angle_Offset;

        /// <summary>
        /// X 上限
        /// </summary>
        public double X_UpLimit;

        /// <summary>
        /// X 下限
        /// </summary>
        public double X_DownLimit;

        /// <summary>
        /// Y 上限
        /// </summary>
        public double Y_UpLimit;

        /// <summary>
        /// Y 下限
        /// </summary>
        public double Y_DownLimit;

        /// <summary>
        /// 角度上限
        /// </summary>
        public double Angle_UpLimit;

        /// <summary>
        /// 角度下限
        /// </summary>
        public double Angle_DownLimit;

        /// <summary>
        /// 读取标定XML文件
        /// </summary>
        /// <param name="Path"></param>
        public void ReafCalibrationXML(string Path)
        {
            using (FileStream fs = new FileStream(Path, FileMode.Open))
            {
                XmlSerializer xml = new XmlSerializer(typeof(CalibrationData));
                m_CalibrationData = xml.Deserialize(fs) as CalibrationData;
                fs.Close();
            }
        }

        //平移旋转标定 图像点角度 -> 物理点角度
        /// <summary>
        /// 平移旋转标定 图像点角度 -> 物理点角度
        /// </summary>
        /// <param name="Angle">d当前角度</param>
        /// <param name="RealTimeX">当前X坐标</param>
        /// <param name="RealTimeY">当前Y坐标</param>
        /// <param name="X">输出X坐标</param>
        /// <param name="Y">输出Y坐标</param>
        /// <param name="AngleDeg">输出角度</param>
        /// <returns></returns>
        public string ApplyCalibration(double Angle, double RealTimeX, double RealTimeY, out double X, out double Y, out double AngleDeg)
        {
            try
            {
                X = 0;
                Y = 0;
                AngleDeg = 0;
                //机械角度转换
                double MachineAngle = Angle * m_CalibrationData.AngleLinearFitFactorK + m_CalibrationData.AngleLinearFitFactorB;
                //string result = MyMathTool.RotateCalculate(m_CalibrationData.ImageKeyPointX, m_CalibrationData.ImageKeyPointY, m_CalibrationData.ImageKeyPointAngle, m_CalibrationData.ImageRotatoCenterX, m_CalibrationData.ImageRotatoCenterY, MachineAngle, out double AngleRotatoCal, out double XAfter, out double YAfter, out double XChange, out double YChange);
                //if (result != "OK")
                //{
                //    return "NG";
                //}
                //double ImageXTransCenter = RealTimeX + (m_CalibrationData.ImageRotatoCenterX - XAfter);
                //double ImageYTransCenter = RealTimeY + (m_CalibrationData.ImageRotatoCenterY - YAfter);
                //HTuple MachineX = new HTuple(), MachineY = new HTuple();
                //MachineX.Dispose(); MachineY.Dispose();
                //HOperatorSet.AffineTransPoint2d(HomMat2D, ImageXTransCenter, ImageYTransCenter, out MachineX, out MachineY);
                //X = MachineX.D;
                //Y = MachineY.D;
                //AngleDeg = Angle;
                return "OK";
            }
            catch
            {
                X = 0;
                Y = 0;
                AngleDeg = 0;
                return "NG";
            }

        }

        public void WriteParameter()
        {
            try
            {
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿上方向标志状态", UpFlagState.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿下方向标志状态", DownFlagState.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿左方向标志状态", LiftFlagStae.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿右方向标志状态", RightFlagState.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "X方向补偿", X_Offset.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "Y方向补偿", Y_Offset.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "角度补偿", Angle_Offset.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "X上限", X_UpLimit.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "X下限", X_DownLimit.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "Y上限", Y_UpLimit.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "Y下限", Y_DownLimit.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "角度上限", Angle_UpLimit.ToString());
                IniAPI.INIWriteValue(RotateUpCameraPath, "上相机旋转纠偏", "角度下限", Angle_DownLimit.ToString());
            }
            catch (Exception ex)
            {
                MessageBox.Show(ex.ToString());
            }
        }

        public void ReadParameter()
        {
            UpFlagState = IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿上方向标志状态", "0");
            DownFlagState = IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿下方向标志状态", "0");
            LiftFlagStae = IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿左方向标志状态", "0");
            RightFlagState = IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "补偿右方向标志状态", "0");
            X_Offset = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "X方向补偿", "0"));
            Y_Offset = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "Y方向补偿", "0"));
            Angle_Offset = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "角度补偿", "0"));
            X_UpLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "X上限", "0"));
            X_DownLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "X下限", "0"));
            Y_UpLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "Y上限", "0"));
            Y_DownLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "Y下限", "0"));
            Angle_UpLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "角度上限", "0"));
            Angle_DownLimit = Convert.ToDouble(IniAPI.INIGetStringValue(RotateUpCameraPath, "上相机旋转纠偏", "角度下限", "0"));
        }
    }
}
